#include "src/scene/kd/kdtree.h"

#include <algorithm>
#include <cmath>
#include <GL/gl.h>

KdTree::KdTree()
{
}

KdTree::~KdTree()
{
    std::cout <<"Existem " << photons.size() << " a serem deletados\nphoton " << std::endl;
//    for(unsigned int i = photons.size(); i > 0; i--)
//    {
//        std::cout <<"\ti - 1 = " << i -1 << std::endl;
//        delete photons[i - 1];
//    }
    std::vector<Photon *>::iterator iter = photons.begin();
    unsigned int i = 0;
    for (iter = photons.begin(); iter != photons.end(); ++iter)
    {
        std::cout <<"  " << i++ << std::flush;
        delete *iter;
    }
    photons.clear();
}

void KdTree::createHeap()
{
    std::vector<Photon *> list(photons);
    balance(&list, 0, photons.size(), 0);

//    std::cout <<"tem " << photons.size() << "photons" << std::endl;
//    int nhe;
//    std::cin >>nhe;

//    print(0, "");
}

void KdTree::print(unsigned int _i, std::string _tab)
{
    if(_i >= photons.size()) return;

    Photon *p = photons[_i];
    std::cout << _tab << _i;

    if(p->flag == X) std::cout << " X: ";
    else if(p->flag == Y) std::cout << " Y: ";
    else std::cout << " Z: ";

    p->position.print();
//    int nhe;
//    std::cin >>nhe;

    print((2*_i)+1, _tab + " ");
    print((2*_i)+2, _tab + " ");

}

unsigned short KdTree::findAxis(std::vector<Photon *> *_list, int _start, int _end)
{
    double minX = INFINITY, minY = INFINITY, minZ = INFINITY, maxX = -INFINITY, maxY = -INFINITY, maxZ = -INFINITY;

    for(int i = _start; i < _end; i++)
    {
        Vector3D pos = (*_list)[i]->position;

        if(pos.x > maxX)
            maxX = pos.x;
        else if(pos.x < minX)
            minX = pos.x;

        if(pos.y > maxY)
            maxY = pos.y;
        else if(pos.y < minY)
            minY = pos.y;

        if(pos.z > maxZ)
            maxZ = pos.z;
        else if(pos.z < minZ)
            minZ = pos.z;
    }

    double disX = (maxX - minX), disY = (maxY - minY), disZ = (maxZ - minZ);

    if(disX > disY)
    {
        if(disX > disZ)
            return X;
        else
            return Z;
    }
    else
    {
        if(disY > disZ)
            return Y;
        else
            return Z;
    }
}

void KdTree::swap(std::vector<Photon *> *_list, unsigned int _i, unsigned int _j)
{
    Photon *aux = (*_list)[_j];
    (*_list)[_j] = (*_list)[_i];
    (*_list)[_i] = aux;
}

int KdTree::partition(std::vector<Photon *> *_list, int _p, int _r, unsigned short _axis)
{
    Photon *x = (*_list)[_r];
    int i = _p - 1;
    for(int j = _p; j < _r; j++)
    {
        if((*_list)[j]->position[_axis] < x->position[_axis])
        {
            i++;
            swap(_list, i, j);
        }
    }
    swap(_list, i + 1, _r);

    return i + 1;
}

void KdTree::quickSort(std::vector<Photon *> *_list, int _p, int _r, unsigned short _axis)
{
//    #if DEBUG
//    std::cout << "\nStart " << _p << " End " << _r << std::endl;
//    #endif //DEBUG

    if (_p < _r){

//        #if DEBUG
//        std::cout << _p << " < " << _r << std::endl;
//        #endif //DEBUG

        int q = partition(_list, _p, _r, _axis);


//        #if DEBUG
//        std::cout << " q " << q << std::endl;
//        #endif //DEBUG

        quickSort(_list, _p, q - 1, _axis);
        quickSort(_list, q + 1, _r, _axis);
    }
//    #if DEBUG
//    else
//        std::cout << _p << " > " << _r <<"  ------ RETORNANDO! "<< std::endl;
//    #endif //DEBUG
}

void KdTree::balance(std::vector<Photon *> *_list, int _start, int _end, unsigned int _index)
{
    if(_end <= _start) return;

    unsigned short axis = findAxis(_list, _start, _end);

//    #if DEBUG
//    std::cout << "Fazendo QuickSort no eixo ";
//    if(axis == X) std::cout << "X: " << std::endl;
//    else if(axis == Y) std::cout << "Y: " << std::endl;
//    else std::cout << "Z: " << std::endl;
//    #endif //DEBUG




    quickSort(_list, _start, _end - 1, axis);

    int median = floor((_start + _end)/2.0);


//    #if DEBUG
//    std::cout << "Depois do quickSort no eixo ";
//    if(axis == X) std::cout << "X: " << std::endl;
//    else if(axis == Y) std::cout << "Y: " << std::endl;
//    else std::cout << "Z: " << std::endl;
//
//    for(int i = _start; i < _end; i++)
//    {
//        std::cout << " " << (*_list)[i]->position[axis];
//    }
//
//    std::cout << std::endl;
//
//
//    std::cout << "Mediana em " << median << ": " << (*_list)[median]->position[axis] << std::endl;
//
//    #endif //DEBUG

    photons[_index] = (*_list)[median];
    photons[_index]->flag = axis;

    balance(_list, _start, median, (2*_index) + 1);
    balance(_list, median + 1, _end, (2*_index) + 2);
}

void KdTree::drawPhotons() const
{
	glPushMatrix();

	glEnable(GL_COLOR_MATERIAL);
	glDisable(GL_LIGHTING);

    glPointSize(2);

    for(unsigned int i = 0; i < photons.size(); i++)
    {
        EnergyColor color = photons[i]->color;
        Vector3D pos = photons[i]->position;

        glBegin(GL_POINTS);
            glColor3d(color.red, color.green, color.blue);
            glVertex3d(pos.x, pos.y, pos.z);
        glEnd();

        Vector3D dir = (photons[i]->direction*0.1 + pos);
        glBegin(GL_LINES);
            glColor3d(color.red, color.green, color.blue);
            glVertex3d(pos.x, pos.y, pos.z);
            glVertex3d(dir.x, dir.y, dir.z);
        glEnd();
//
//        dir = (photons[i]->goingTo*0.3 + pos);
//        glBegin(GL_LINES);
//            glColor3d(color.red, color.green, color.blue);
//            glVertex3d(pos.x, pos.y, pos.z);
//            glVertex3d(dir.x, dir.y, dir.z);
//        glEnd();
    }


    glPointSize(1);

    glEnable(GL_LIGHTING);

    glDisable(GL_COLOR_MATERIAL);


	glPopMatrix();
}

void KdTree::locateNearestPhotons(const unsigned int &_p, const double &_maxPhotons, const double &_maxRadius, const Vector3D &_point, std::vector<NearestPhoton> *_heap) const
{

    Photon *photon = photons[_p];

//    std::cout << "\n\nPhoton " << _p << " no plano ";
//    if(photon->flag == X) std::cout << "X: " << std::endl;
//    else if(photon->flag == Y) std::cout << "Y: " << std::endl;
//    else std::cout << "Z: " << std::endl;
//    std::cout << " de posicao = "; photon->position.print();
//    std::cout << "\t\tSua real distancia é " << (photon->position - _point).length_2() << std::endl;

    if(2*_p + 2 < photons.size())
    {


        double axisDelta = _point[photon->flag] - photon->position[photon->flag];

//        std::cout << "\tesse photon tem os dois filhos \n\te tem uma distancia no eixo do ponto de " << axisDelta << std::endl;

        double d2 = _maxRadius*_maxRadius;
        if(_heap->size() >= _maxPhotons)
            d2 = _heap->front().distance2;
        //o ponto está  esquerda dd nó, que é onde deve seguir o caminho na kd-tree
        if(axisDelta > 0.0)
        {
            locateNearestPhotons((2*_p) + 2, _maxPhotons, _maxRadius, _point, _heap);

            if(axisDelta*axisDelta < d2)
                locateNearestPhotons((2*_p) + 1, _maxPhotons, _maxRadius, _point, _heap);
        }
        else
        {
            locateNearestPhotons((2*_p) + 1, _maxPhotons, _maxRadius, _point, _heap);

            if(axisDelta*axisDelta < d2)
                locateNearestPhotons((2*_p) + 2, _maxPhotons, _maxRadius, _point, _heap);
        }
    }


    double distance2 = (photon->position - _point).length_2();

//    std::cout << "\n------ Checando photon " << _p << " de posicao "; photon->position.print();

    if(_heap->size() < _maxPhotons)
    {
        //ainda pode colcoar sem retirar os existentes
        if(distance2 < _maxRadius*_maxRadius)
        {
//            std::cout << "\t\t\t  Vai inseir sem remover" << std::endl;
            _heap->push_back(NearestPhoton(photon, distance2, _maxRadius));
            std::push_heap(_heap->begin(),_heap->end());
        }
    }
    else
    {
        //tem que remover o maximo, para poder inserir o proximo
        if(distance2 < _heap->front().distance2)
        {
//            std::cout << "\t\t\t  Vai inseir removendo o ultimo" << std::endl;

            std::pop_heap(_heap->begin(),_heap->end());
            _heap->pop_back();

            _heap->push_back(NearestPhoton(photon, distance2, _maxRadius));
            std::push_heap(_heap->begin(),_heap->end());
        }
    }
}
